#!/usr/bin/env python
# -*- coding: utf-8 -*-
'''
@Time    :   2020/10/21 09:54:14
@Author  :   Tianyi Wu 
@Contact :   wutianyi@hotmail.com
@File    :   fetch_movebase_action.py
@Version :   1.0
@Desc    :   None
'''

# here put the import lib
from __future__ import print_function
import os
import sys
import actionlib
from actionlib import action_server
from actionlib.simple_action_client import SimpleGoalState
import rospy
from math import sin, cos
from std_srvs.srv import Empty
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from rosplan_pytools import rosplan
from rosplan_pytools.rosplan.interfaces import action_interface
from rosplan_pytools.rosplan.controller import knowledge_base as kb
from rosplan_dispatch_msgs.msg import ActionFeedback, ActionDispatch
from actionlib_msgs.msg import GoalStatus


def get_move_goal(x, y, theta, frame="map"):
    move_goal = MoveBaseGoal()
    move_goal.target_pose.pose.position.x = x
    move_goal.target_pose.pose.position.y = y
    move_goal.target_pose.pose.orientation.z = sin(theta / 2.0)
    move_goal.target_pose.pose.orientation.w = cos(theta / 2.0)
    move_goal.target_pose.header.frame_id = frame
    move_goal.target_pose.header.stamp = rospy.Time.now()

    return move_goal


class fetch_movebase_action(action_interface.Action):
    name = 'goto_waypoint'

    def __init__(self, action_id, dispatch_time, feedback_pub, arguments):
        super(fetch_movebase_action, self).__init__(action_id, dispatch_time,
                                                    feedback_pub, arguments)

        self.waypoints = rospy.get_param('/rosplan_demo_waypoints/wp')

    def _start(self, **kwargs):

        rospy.loginfo(
            '-----------------------------------------------------')
        rospy.loginfo('start fetch movebase action, actionid: {}'.format(
            self.action_id))
        waypoint_key = self.arguments['to']
        rospy.loginfo('waypoint_key: {}'.format(waypoint_key))
        waypoint_value = self.waypoints[waypoint_key]
        rospy.loginfo('waypoint: x={}, y={}, z={}'.format(
            waypoint_value[0], waypoint_value[1], waypoint_value[2]))

        client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
        rospy.loginfo("Waiting for move_base...")
        client.wait_for_server()

        move_goal = get_move_goal(waypoint_value[0], waypoint_value[1],
                                  waypoint_value[2])

        client.send_goal(move_goal)
        finished_before_timeout = client.wait_for_result()
        if finished_before_timeout:
            state = client.get_state()
            rospy.loginfo("{} action finished: {}".format(fetch_movebase_action.name, state));
            if state == GoalStatus.SUCCEEDED:
                self._report_success()
                return True
            else:
                rospy.wait_for_service("/move_base/clear_costmaps")
                try:
                    rospy.ServiceProxy("/move_base/clear_costmaps", Empty)
                except rospy.ServiceException as e:
                    rospy.logerr("Service call failed: %s" % e)
                return False
        else:
            client.cancel_all_goals()
            rospy.loginfo("{} action timed out".format(
                fetch_movebase_action.name))
            self._report_failed()
            return False
        # action_interface.CheckActionAndProcessEffects(
        #     fetch_movebase_action.name)


if __name__ == "__main__":

    try:

        rospy.init_node("fetch_rosplan_movebase")
        # rosplan.init()
        kb.initialize()

        # Make sure sim time is working
        while not rospy.Time.now():
            pass

        REGISTER_ACTIONS_AUTOMATICALLY = False
        action_interface.register_action(fetch_movebase_action.name,
                                         fetch_movebase_action)
        action_interface.initialize_actions(REGISTER_ACTIONS_AUTOMATICALLY)
        action_interface.start_actions(
            dispatch_topic_name='/rosplan_plan_dispatcher/action_dispatch',
            feedback_topic_name='/rosplan_plan_dispatcher/action_feedback')

        # rospy.Subscriber('/rosplan_plan_dispatcher/action_dispatch', ActionDispatch, callback=action_callback)
        # print(rospy.get_param('/rosplan_demo_waypoints/waypoint_frameid'))
        rospy.spin()

    except rospy.ROSInterruptException:
        pass
